#pragma once

#include "BasicDefs.h"
#include "A_star.h"

namespace VoxelCore{

	using namespace std;

	//ascending
	enum VoxelState {Occupied = 10, Unknow = -1, Useless, WheelChair, Child, Adult1, Adult2, Fly, Path = 20};
	const Indice3i OUTOFRANGE = Indice3i(-1,-1,-1);
	const Indice3i POSITIVE_Z = Indice3i(0, 0, 1);
	const Indice3i NEGATIVE_Z = Indice3i(0, 0, -1);
	const Indice3i POSITIVE_Y = Indice3i(0, 1, 0);
	const Indice3i NEGATIVE_Y = Indice3i(0, -1, 0);
	const Indice3i POSITIVE_X = Indice3i(1, 0, 0);
	const Indice3i NEGATIVE_X = Indice3i(-1, 0, 0);

	class CVoxel
	{
	private:
		//properties
		char State;
	public:
		//construction
		CVoxel(){State = Unknow;}
		CVoxel(char chTag):State(chTag){}
		virtual ~CVoxel(){};

	public:
		//operations
		void SetState(char);
		//
		char GetState();
	};

	typedef shared_ptr<CVoxel> ptr_CVoxel;
	typedef vector<ptr_CVoxel> VoxelList1;
	typedef vector<VoxelList1> VoxelList2;
	typedef vector<VoxelList2> VoxelList3;

	class CVoxelCore  
	{
	private:

		//Data
		VoxelList3 Voxels;

		//voxel size
		double dVoxelSize;

		//Voxel Extent
		Indice3i iExtent;

		//spatial extent
		Point3d ptOrigin;

	private:
		//manipulate operations
		Indice3i RegularPos(const Indice3i&);
		//
		//the second value should be normalized
		Indice3i FindNeighborEx(const Indice3i&, const Indice3i&, int);
		//the second value should be normalized
		Indice3i search_least_voxel(const Indice3i& , const Indice3i&, int );
		//the second value should be normalized
		void search_set_const_voxel_state(const Indice3i&, const Indice3i&, char);
		void search_set_weight_voxel_state(const Indice3i&, const Indice3i&, const Indice3i&, char, int);
		//////////////////////////////////////////////////////////////////////////
		//A star path finding
		//g_score part should be adjusted
		float heuristic_H_cost_estimate(const Indice3i& , const Indice3i& );
		int heuristic_G_cost_estimate(const Indice3i& , const Indice3i&, int);
		shared_ptr<Indice_cost_pair_list> find_neighbor_Cost(const Indice_cost_pair& , const Indice3i& , int );
		bool update_openset_by_less_cost(const Indice_cost_pair& , Indice_cost_pair_pqueue&);
		bool update_closeset_by_less_cost(const Indice_cost_pair& , Indice_cost_pair_vector&, Indice_cost_pair& );
		void construct_path(Indice_pair_map& , long , shared_ptr<IndicesList> );
		long Indice3ToIndex(const Indice3i&);
		Indice3i IndexToIndice3(const long& );
	public:
		//////////////////////////////////////////////////////////////////////////
		//generic Operations
		ptr_CVoxel GetVoxel(const Indice3i&);
		Point3d GetCenterCoord(const Indice3i&);
		//Query operations
		Indice3i QuerybyPointEx(const Point3d&);//the result should be checked by RegularPos
		ptr_CVoxel QuerybyPoint(const Point3d&);
		shared_ptr<IndicesList> QuerybyBbox(const BoundingBox&);
		shared_ptr<IndicesList> QuerybyShell(const Polyhedron3&, const Pointlist&);
		shared_ptr<IndicesList> QuerybySurface(const Surface3&, const Pointlist&);
		shared_ptr<IndicesList> QueryPathUsingAstar(const Indice3i&, const Indice3i&, int);

		//Output voxels to vtk
		bool WriteVoxels(string);
		//Ouput path to obj
		bool WritePath(string, shared_ptr<IndicesList>);
		//////////////////////////////////////////////////////////////////////////
		//Specified operations
		bool TagVoxelbyFloorandCeiling(int step);
		
	public:
		//construction
		CVoxelCore();
		CVoxelCore(const Pointlist&, double);
		virtual ~CVoxelCore();
	};
}



















